#include "rtk.h"

//无参数实现单点定位
RTK::RTK()
{
    //RTK解算参数
    prcopt_t prcopt = prcopt_default;
    prcopt.mode  = PMODE_SINGLE;
    prcopt.navsys = SYS_GPS|SYS_CMP/*|SYS_GLO|SYS_CMP*/;
    prcopt.nf = 3;	     //(1:l1,2:l1+l2,3:l1+l2+l5)
    prcopt.soltype = 0;  // solution type (0:forward,1:backward,2:combined)
    prcopt.modear = ARMODE_CONT;	 //* AR mode (0:off,1:continuous,2:instantaneous,3:fix and hold,4:ppp-ar) */
    prcopt.glomodear = 0;
    prcopt.bdsmodear = 1;
    prcopt.dynamics = 0;
    prcopt.elmin = 15*D2R;
    prcopt.sateph = EPHOPT_BRDC;
    prcopt.ionoopt = IONOOPT_BRDC;
    prcopt.tropopt = TROPOPT_SAAS;
    //prcopt.refpos = 0;//需要给基准站坐标
    //sscanf_s(pos, "%lf,%lf,%lf", &prcopt.rb[0],&prcopt.rb[1],&prcopt.rb[2]);

    //将prcopt配置参数赋值给rtk结构体
    rtkinit(&rtk,&prcopt);

    //基准站和流动站坐标
    for (int i=0;i<6;i++)
    {
        rtk.rb[i]=i<3?prcopt.rb[i]:0.0;
    }

    //nav结构体开辟空间并初始化
    eph_t eph0 ={0,-1,-1};
    if (!(nav.eph = (eph_t *)malloc(sizeof(eph_t)*MAXSAT*2))) return;
    for (int i=0;i<MAXSAT *2;i++) nav.eph[i]=eph0;
    nav.n = MAXSAT *2;
}
//初始化一些不变参数，如解算参数和基准站坐标等。
RTK::RTK(char* pos)
{
    //RTK解算参数
    prcopt_t prcopt = prcopt_default;
    prcopt.mode  = PMODE_KINEMA;
    prcopt.navsys = SYS_GPS|SYS_CMP/*|SYS_GLO|SYS_CMP*/;
    prcopt.nf = 3;	     //(1:l1,2:l1+l2,3:l1+l2+l5)
    prcopt.soltype = 0;  // solution type (0:forward,1:backward,2:combined)
    prcopt.modear = ARMODE_CONT;	 //* AR mode (0:off,1:continuous,2:instantaneous,3:fix and hold,4:ppp-ar) */
    prcopt.glomodear = 0;
    prcopt.bdsmodear = 1;
    prcopt.dynamics = 0;
    prcopt.elmin = 15*D2R;
    prcopt.sateph = EPHOPT_BRDC;
    prcopt.ionoopt = IONOOPT_BRDC;
    prcopt.tropopt = TROPOPT_SAAS;
    prcopt.refpos = 0;//需要给基准站坐标
    sscanf_s(pos, "%lf,%lf,%lf", &prcopt.rb[0],&prcopt.rb[1],&prcopt.rb[2]);

    //将prcopt配置参数赋值给rtk结构体
    rtkinit(&rtk,&prcopt);

    //基准站和流动站坐标
    for (int i=0;i<6;i++)
    {
        rtk.rb[i]=i<3?prcopt.rb[i]:0.0;
    }

    //nav结构体开辟空间并初始化
    eph_t eph0 ={0,-1,-1};
    if (!(nav.eph = (eph_t *)malloc(sizeof(eph_t)*MAXSAT*2))) return;
    for (int i=0;i<MAXSAT *2;i++) nav.eph[i]=eph0;
    nav.n = MAXSAT *2;
}

void RTK::updatenav(nav_t *nav)
{
    int i,j;
    for (i=0;i<MAXSAT;i++) for (j=0;j<NFREQ;j++)
    {
        nav->lam[i][j]=satwavelen(i+1,j,nav);
    }
}
void RTK::prsolution(const sol_t *sol)
{
    const char *solstr[]={"------","FIX","FLOAT","SBAS","DGPS","SINGLE","PPP",""};
    double Qr[9];
    char tstr[64]="";

//    if (sol->time.time==0||!sol->stat) return;

    time2str(gpst2utc(sol->time),tstr,1);
    printf("%s (%-6s)", tstr,solstr[sol->stat]);

    Qr[0]=sol->qr[0];
    Qr[4]=sol->qr[1];
    Qr[8]=sol->qr[2];
    Qr[1]=Qr[3]=sol->qr[3];
    Qr[5]=Qr[7]=sol->qr[4];
    Qr[2]=Qr[6]=sol->qr[5];

    printf(" X:%12.3f",sol->rr[0]);
    printf(" Y:%12.3f",sol->rr[1]);
    printf(" Z:%12.3f",sol->rr[2]);
    printf(" Vx:%6.3f",sol->rr[3]);
    printf(" Vy:%6.3f",sol->rr[4]);
    printf(" Vz:%6.3f",sol->rr[5]);

    printf(" A:%4.1f R:%5.1f N:%2d",sol->age,sol->ratio,sol->ns);
    printf("\n");
}


//获得基准站和流动站观测值并解码
int RTK::GetRovObs(char* chRover)
{
    obsd_t tmp[MAXOBS*2];
    obs.n=0;
    obs.data=tmp;

    int nPrn=0;
    long time=0;
    int i=0;
    char satid[3]="";//卫星号G03
    char *p1,*p2;

    //流动站观测值解码
    p1 = chRover+10;
    sscanf_s(p1,"%d",&nPrn);
    p2=strstr(p1,"$");
    sscanf_s(p2+1,"%ld",&time);
    while((p2=strstr(p1,";")) != NULL)//没颗卫星的观测值以“;”分割
    {
        sscanf_s(p2+1,"%3c,",satid,3);
        sscanf_s(p2+5,"%lf,%lf,%lf,%lf,",&tmp[i].P[0],&tmp[i].L[0],&tmp[i].P[1],&tmp[i].L[1]);
        tmp[i].sat=(unsigned char)satid2no(satid);
        tmp[i].time.time=time;
        tmp[i].rcv=1;//流动站

        obs.data[obs.n++] = tmp[i];//流动站

        i++;
        p1=p2+1;
    }

    return 0;
}

//获得基准站和流动站观测值并解码
int RTK::GetObs(char* chBase,char* chRover)
{
    obsd_t tmp[MAXOBS*2];
    obs.n=0;
    obs.data=tmp;

    int nPrn=0;
    long time=0;
    int i=0;
    char satid[3]="";//卫星号G03
    char *p1,*p2;

    //流动站观测值解码
    p1 = chRover+10;
    sscanf_s(p1,"%d",&nPrn);
    p2=strstr(p1,"$");
    sscanf_s(p2+1,"%ld",&time);
    while((p2=strstr(p1,";")) != NULL)//没颗卫星的观测值以“;”分割
    {
        sscanf_s(p2+1,"%3c,",satid,3);
        sscanf_s(p2+5,"%lf,%lf,%lf,%lf,",&tmp[i].P[0],&tmp[i].L[0],&tmp[i].P[1],&tmp[i].L[1]);
        tmp[i].sat=(unsigned char)satid2no(satid);
        tmp[i].time.time=time;
        tmp[i].rcv=1;//流动站

        obs.data[obs.n++] = tmp[i];//流动站

        i++;
        p1=p2+1;
    }

    //基准站观测值解码
    p1 = chBase+10;
    sscanf_s(p1,"%d",&nPrn);
    p2=strstr(p1,"$");
    sscanf_s(p2+1,"%ld",&time);

    while((p2=strstr(p1,";")) != NULL)//没颗卫星的观测值以“;”分割
    {
        sscanf_s(p2+1,"%3c,",satid,3);
        //sscanf_s(p2+5,"%lf,",&tmp[i].P[0]);
        sscanf_s(p2+5,"%lf,%lf,%lf,%lf,",&tmp[i].P[0],&tmp[i].L[0],&tmp[i].P[1],&tmp[i].L[1]);
        tmp[i].sat=(unsigned char)satid2no(satid);
        tmp[i].time.time=time;
        tmp[i].rcv=2;//基准站
        obs.data[obs.n++] = tmp[i];

        i++;
        p1=p2+1;
    }

    return 0;
}

int RTK::GetNav(char* chNav)
{
    eph_t eph0={0};
    geph_t geph0={0};
    char *p1,*p2;
    char satid[4]={0};
    long toe_time,tof_time,toc_time,ttr_time;
    int prn=0;

    p1 = chNav;

    p2=strstr(p1,"$");

    sscanf_s(p2+1,"%3c,",satid,3);

    int sat=satid2no(satid);
    if (sat ==0)
        return 0;
    if (satsys(sat,&prn)==SYS_GLO)
    {
        nav.geph[prn-1]=geph0;
        nav.geph[prn-1].sat=sat;
        toe_time=tof_time=0;
        sscanf(p2+5,"%d,%d,%d,%d,%d,%ld,%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf",
               &nav.geph[prn-1].iode,&nav.geph[prn-1].frq,&nav.geph[prn-1].svh,
               &nav.geph[prn-1].sva,&nav.geph[prn-1].age,&toe_time,&tof_time,
               &nav.geph[prn-1].pos[0],&nav.geph[prn-1].pos[1],&nav.geph[prn-1].pos[2],
               &nav.geph[prn-1].vel[0],&nav.geph[prn-1].vel[1],&nav.geph[prn-1].vel[2],
               &nav.geph[prn-1].acc[0],&nav.geph[prn-1].acc[1],&nav.geph[prn-1].acc[2],
               &nav.geph[prn-1].taun  ,&nav.geph[prn-1].gamn  ,&nav.geph[prn-1].dtaun);
        nav.geph[prn-1].toe.time=toe_time;
        nav.geph[prn-1].tof.time=tof_time;
        updatenav(&nav);
    }
    else
    {
        nav.eph[sat-1]=eph0;
        nav.eph[sat-1].sat=sat;
        toe_time=toc_time=ttr_time=0;
        sscanf(p2+5,"%d,%d,%d,%d,%ld,%ld,%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%d,%d",
               &nav.eph[sat-1].iode,&nav.eph[sat-1].iodc,&nav.eph[sat-1].sva ,
               &nav.eph[sat-1].svh ,&toe_time,&toc_time,&ttr_time,
               &nav.eph[sat-1].A   ,&nav.eph[sat-1].e   ,&nav.eph[sat-1].i0  ,
               &nav.eph[sat-1].OMG0,&nav.eph[sat-1].omg ,&nav.eph[sat-1].M0  ,
               &nav.eph[sat-1].deln,&nav.eph[sat-1].OMGd,&nav.eph[sat-1].idot,
               &nav.eph[sat-1].crc ,&nav.eph[sat-1].crs ,&nav.eph[sat-1].cuc ,
               &nav.eph[sat-1].cus ,&nav.eph[sat-1].cic ,&nav.eph[sat-1].cis ,
               &nav.eph[sat-1].toes,&nav.eph[sat-1].fit ,&nav.eph[sat-1].f0  ,
               &nav.eph[sat-1].f1  ,&nav.eph[sat-1].f2  ,&nav.eph[sat-1].tgd[0],
               &nav.eph[sat-1].code, &nav.eph[sat-1].flag);
        nav.eph[sat-1].toe.time=toe_time;
        nav.eph[sat-1].toc.time=toc_time;
        nav.eph[sat-1].ttr.time=ttr_time;
        updatenav(&nav);
    }

    return 1;
}


int RTK::RtkPos(char* result)
{
    //3、RTK定位
    rtkpos(&rtk,obs.data,obs.n,&nav);//主要定位函数
    prsolution(&rtk.sol);

    return 0;
}

int RTK::SinglePos(char* result)
{
    //3、RTK定位
    rtkpos(&rtk,obs.data,obs.n,&nav);//主要定位函数
    prsolution(&rtk.sol);

    return 0;
}
